#include "UtilityPCH.h"

#include "UT_Transform.h"
#include "UT_MathHelper.h"

using namespace Utility;
//----------------------------------------------------------------------------
// UT_Transform
//----------------------------------------------------------------------------
UT_Transform::UT_Transform( const float mat[4][4] )
{
	m = UT_Matrix4x4Float(mat[0][0], mat[0][1], mat[0][2], mat[0][3],
		mat[1][0], mat[1][1], mat[1][2], mat[1][3],
		mat[2][0], mat[2][1], mat[2][2], mat[2][3],
		mat[3][0], mat[3][1], mat[3][2], mat[3][3]);
	mInv = UT_MathHelper::Inverse(m);
}
//----------------------------------------------------------------------------
UT_Transform::UT_Transform(const UT_Matrix4x4Float &mat)
: m(mat), mInv(UT_MathHelper::Inverse(mat)) 
{

}
//------------------------------------------------------------
UT_BBOX_AABB UT_Transform::operator()(const UT_BBOX_AABB &b) const 
{
	const UT_Transform &M = *this;
	UT_BBOX_AABB ret(M(UT_Point3Float(b.pMin.x, b.pMin.y, b.pMin.z)));
	ret = UT_MathHelper::Union(ret, M(UT_Point3Float(b.pMax.x, b.pMin.y, b.pMin.z)));
	ret = UT_MathHelper::Union(ret, M(UT_Point3Float(b.pMin.x, b.pMax.y, b.pMin.z)));
	ret = UT_MathHelper::Union(ret, M(UT_Point3Float(b.pMin.x, b.pMin.y, b.pMax.z)));
	ret = UT_MathHelper::Union(ret, M(UT_Point3Float(b.pMin.x, b.pMax.y, b.pMax.z)));
	ret = UT_MathHelper::Union(ret, M(UT_Point3Float(b.pMax.x, b.pMax.y, b.pMin.z)));
	ret = UT_MathHelper::Union(ret, M(UT_Point3Float(b.pMax.x, b.pMin.y, b.pMax.z)));
	ret = UT_MathHelper::Union(ret, M(UT_Point3Float(b.pMax.x, b.pMax.y, b.pMax.z)));
	return ret;
}
//----------------------------------------------------------------------------
UT_Transform UT_Transform::operator*(const UT_Transform &t2) const 
{
	UT_Matrix4x4Float m1 = UT_MathHelper::Mul(m, t2.m);
	UT_Matrix4x4Float m2 = UT_MathHelper::Mul(t2.mInv, mInv);
	return UT_Transform(m1, m2);
}
//----------------------------------------------------------------------------
bool UT_Transform::SwapsHandedness() const 
{
	float det = ((m.m[0][0] *
		(m.m[1][1] * m.m[2][2] -
		m.m[1][2] * m.m[2][1])) -
		(m.m[0][1] *
		(m.m[1][0] * m.m[2][2] -
		m.m[1][2] * m.m[2][0])) +
		(m.m[0][2] *
		(m.m[1][0] * m.m[2][1] -
		m.m[1][1] * m.m[2][0])));
	return det < 0.f;
}
//----------------------------------------------------------------------------
// UT_AnimatedTransform
//----------------------------------------------------------------------------
UT_AnimatedTransform::UT_AnimatedTransform(const UT_Transform *transform1, float time1,const UT_Transform *transform2, float time2)
: startTime(time1)
, endTime(time2)
, startTransform(transform1)
, endTransform(transform2)
, actuallyAnimated(*startTransform != *endTransform) 
{
	UT_MathHelper::Decompose(startTransform->m, &T[0], &R[0], &S[0]);
	UT_MathHelper::Decompose(endTransform->m, &T[1], &R[1], &S[1]);
}
//----------------------------------------------------------------------------
void UT_AnimatedTransform::Interpolate(float time, UT_Transform *t) const 
{
	// Handle boundary conditions for matrix interpolation
	if (!actuallyAnimated || time <= startTime) 
	{
		*t = *startTransform;
		return;
	}
	if (time >= endTime) 
	{
		*t = *endTransform;
		return;
	}
	
	float dt = (time - startTime) / (endTime - startTime);

	// Interpolate translation at _dt_
	UT_Vec3Float trans		= UT_MathHelper::Lerp(dt, T[0], T[1]);

	// Interpolate rotation at _dt_
	UT_Quaternions rotate	= UT_MathHelper::Lerp(dt, R[0], R[1]);

	// Interpolate scale at _dt_
	UT_Matrix4x4Float scale;
	for (int i = 0; i < 3; ++i)
		for (int j = 0; j < 3; ++j)
			scale.m[i][j] = UT_MathHelper::Lerp(dt, S[0].m[i][j], S[1].m[i][j]);

	// Compute interpolated matrix as product of interpolated components
	*t = UT_MathHelper::MakeTransform_Translate(trans) * rotate.ToTransform() * UT_Transform(scale);
}
//----------------------------------------------------------------------------
void UT_AnimatedTransform::operator()(const UT_Ray &r, UT_Ray *tr) const 
{
	if (!actuallyAnimated || r.time <= startTime)
		(*startTransform)(r, tr);
	else if (r.time >= endTime)
		(*endTransform)(r, tr);
	else 
	{
		UT_Transform t;
		Interpolate(r.time, &t);
		t(r, tr);
	}
	tr->time = r.time;
}
//----------------------------------------------------------------------------
void UT_AnimatedTransform::operator()(const UT_RayDifferential &r,UT_RayDifferential *tr) const 
{
	if (!actuallyAnimated || r.time <= startTime)
		(*startTransform)(r, tr);
	else if (r.time >= endTime)
		(*endTransform)(r, tr);
	else 
	{
		UT_Transform t;
		Interpolate(r.time, &t);
		t(r, tr);
	}
	tr->time = r.time;
}
//----------------------------------------------------------------------------
UT_Point3Float UT_AnimatedTransform::operator()(float time, const UT_Point3Float &p) const
{
	if (!actuallyAnimated || time <= startTime)
		return (*startTransform)(p);
	else if (time >= endTime)
		return (*endTransform)(p);
	UT_Transform t;
	Interpolate(time, &t);
	return t(p);
}
//----------------------------------------------------------------------------
UT_Vec3Float UT_AnimatedTransform::operator()(float time, const UT_Vec3Float &v) const 
{
	if (!actuallyAnimated || time <= startTime)
		return (*startTransform)(v);
	else if (time >= endTime)
		return (*endTransform)(v);
	UT_Transform t;
	Interpolate(time, &t);
	return t(v);
}
//----------------------------------------------------------------------------
UT_BBOX_AABB UT_AnimatedTransform::MotionBounds(const UT_BBOX_AABB &b, bool useInverse) const 
{
	if (!actuallyAnimated)
	{
		if(useInverse) 
			return UT_MathHelper::Inverse(*startTransform)(b);
		else 
			return (*startTransform)(b);
	}

	UT_BBOX_AABB ret;
	const int nSteps = 128;
	for (int i = 0; i < nSteps; ++i) 
	{
		UT_Transform t;
		float time = UT_MathHelper::Lerp(float(i)/float(nSteps-1), startTime, endTime);
		Interpolate(time, &t);
		if (useInverse) t = UT_MathHelper::Inverse(t);
		ret = UT_MathHelper::Union(ret, t(b));
	}
	
	return ret;
}